#include "btPhoneMode.h"
#include "mainControl.h"
#include "sdkconfig.h"
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "audio_element.h"
#include "audio_pipeline.h"
#include "audio_event_iface.h"
#include "audio_common.h"
#include "audio_hal.h"
#include "esp_peripherals.h"
#include "board.h"
#include "i2s_stream.h"
#include "raw_stream.h"
#include "periph_button.h"

#include "esp_bt.h"
#include "esp_gap_ble_api.h"
#include "esp_gap_bt_api.h"
#include "esp_gatts_api.h"
#include "esp_bt_defs.h"
#include "esp_bt_main.h"

#include "filter_resample.h"
#include "math.h"
#include "equalizer.h"

typedef enum {
    PERIPH_BLUETOOTH_UNKNOWN = 0,       /*!< No event */
    PERIPH_BLUETOOTH_CONNECTED,         /*!< A bluetooth device was connected */
    PERIPH_BLUETOOTH_DISCONNECTED,      /*!< Last connection was disconnected */
} periph_bluetooth_event_id_t;

#define GATTS_TABLE_TAG "GATTS_SPP"

#define SPP_PROFILE_NUM 1
#define SPP_PROFILE_APP_IDX 0
#define ESP_SPP_APP_ID 0x56
#define SAMPLE_DEVICE_NAME "ESP_SPP_SER001"
#define SPP_SVC_INST_ID 0

/// SPP Service
static const uint16_t spp_service_uuid = 0xABF0;
/// Characteristic UUID
#define ESP_GATT_UUID_SPP_DATA_RECEIVE 0xABF1
#define ESP_GATT_UUID_SPP_DATA_NOTIFY 0xABF2
#define ESP_GATT_UUID_SPP_COMMAND_RECEIVE 0xABF3
#define ESP_GATT_UUID_SPP_COMMAND_NOTIFY 0xABF4

#ifdef SUPPORT_HEARTBEAT
#define ESP_GATT_UUID_SPP_HEARTBEAT 0xABF5
#endif

static const char *TAG = "WIRED_MODE";

static const uint8_t spp_adv_data[23] = {
    0x02, 0x01, 0x06,
    0x03, 0x03, 0xF0, 0xAB,
    //0x0F, 0x09, 0x45, 0x53, 0x50, 0x5f, 0x53, 0x50, 0x50, 0x5f, 0x53, 0x45, 0x52, 0x30, 0x30, 0x31};
    // 0x0F, 0x09, 'E', 'S', 'P',   '_',  'S',  'P',  'P',  '-',  'S', 'E', 'R', '0', '0', '1'
    0x0F, 0x09, 'K', 'Z', 'J', '_', 'S', 'P', 'P', '_', '0', '0', '0', '0', '0', '0'};  

static uint16_t spp_mtu_size = 263;
static uint16_t spp_conn_id = 0xffff;
static esp_gatt_if_t spp_gatts_if = 0xff;
QueueHandle_t spp_uart_queue = NULL;
static xQueueHandle bt_cmd_queue = NULL;

#ifdef SUPPORT_HEARTBEAT
static xQueueHandle cmd_heartbeat_queue = NULL;
static uint8_t heartbeat_s[9] = {'i', 'S', 't', 'e', 'T', 'h', 'o', 'e', 's'};
static bool enable_heart_ntf = false;
static uint8_t heartbeat_count_num = 0;
#endif

static bool enable_data_ntf = false;
static bool enable_cmd_ntf = false;

static bool is_connected = false;
static bool is_exit = false;
static esp_bd_addr_t spp_remote_bda = {
    0x0,
};

static uint16_t spp_handle_table[SPP_IDX_NB];

//  0.625us;     160 = 100ms, 1600 = 1s
static esp_ble_adv_params_t spp_adv_params = {
    .adv_int_min = 0x20,
    .adv_int_max = 0x40,
    .adv_type = ADV_TYPE_IND,
    .own_addr_type = BLE_ADDR_TYPE_PUBLIC,
    .channel_map = ADV_CHNL_ALL,
    .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY,
};

struct gatts_profile_inst
{
    esp_gatts_cb_t gatts_cb;
    uint16_t gatts_if;
    uint16_t app_id;
    uint16_t conn_id;
    uint16_t service_handle;
    esp_gatt_srvc_id_t service_id;
    uint16_t char_handle;
    esp_bt_uuid_t char_uuid;
    esp_gatt_perm_t perm;
    esp_gatt_char_prop_t property;
    uint16_t descr_handle;
    esp_bt_uuid_t descr_uuid;
};

typedef struct spp_receive_data_node
{
    int32_t len;
    uint8_t *node_buff;
    struct spp_receive_data_node *next_node;
} spp_receive_data_node_t;

static spp_receive_data_node_t *temp_spp_recv_data_node_p1 = NULL;
static spp_receive_data_node_t *temp_spp_recv_data_node_p2 = NULL;

typedef struct spp_receive_data_buff
{
    int32_t node_num;
    int32_t buff_size;
    spp_receive_data_node_t *first_node;
} spp_receive_data_buff_t;

static spp_receive_data_buff_t SppRecvDataBuff = {
    .node_num = 0,
    .buff_size = 0,
    .first_node = NULL};

static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param);

/* One gatt-based profile one app_id and one gatts_if, this array will store the gatts_if returned by ESP_GATTS_REG_EVT */
static struct gatts_profile_inst spp_profile_tab[SPP_PROFILE_NUM] = {
    [SPP_PROFILE_APP_IDX] = {
        .gatts_cb = gatts_profile_event_handler,
        .gatts_if = ESP_GATT_IF_NONE, /* Not get the gatt_if, so initial is ESP_GATT_IF_NONE */
    },
};

/*
 *  SPP PROFILE ATTRIBUTES
 ****************************************************************************************
 */

#define CHAR_DECLARATION_SIZE (sizeof(uint8_t))
static const uint16_t primary_service_uuid = ESP_GATT_UUID_PRI_SERVICE;
static const uint16_t character_declaration_uuid = ESP_GATT_UUID_CHAR_DECLARE;
static const uint16_t character_client_config_uuid = ESP_GATT_UUID_CHAR_CLIENT_CONFIG;

static const uint8_t char_prop_read_notify = ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_NOTIFY;
static const uint8_t char_prop_read_write = ESP_GATT_CHAR_PROP_BIT_WRITE_NR | ESP_GATT_CHAR_PROP_BIT_READ;

#ifdef SUPPORT_HEARTBEAT
static const uint8_t char_prop_read_write_notify = ESP_GATT_CHAR_PROP_BIT_READ | ESP_GATT_CHAR_PROP_BIT_WRITE_NR | ESP_GATT_CHAR_PROP_BIT_NOTIFY;
#endif

///SPP Service - data receive characteristic, read&write without response
static const uint16_t spp_data_receive_uuid = ESP_GATT_UUID_SPP_DATA_RECEIVE;
static const uint8_t spp_data_receive_val[20] = {0x00};

///SPP Service - data notify characteristic, notify&read
static const uint16_t spp_data_notify_uuid = ESP_GATT_UUID_SPP_DATA_NOTIFY;
static const uint8_t spp_data_notify_val[20] = {0x00};
static const uint8_t spp_data_notify_ccc[2] = {0x00, 0x00};

///SPP Service - command characteristic, read&write without response
static const uint16_t spp_command_uuid = ESP_GATT_UUID_SPP_COMMAND_RECEIVE;
static const uint8_t spp_command_val[10] = {0x00};

///SPP Service - status characteristic, notify&read
static const uint16_t spp_status_uuid = ESP_GATT_UUID_SPP_COMMAND_NOTIFY;
static const uint8_t spp_status_val[10] = {0x00};
static const uint8_t spp_status_ccc[2] = {0x00, 0x00};

#ifdef SUPPORT_HEARTBEAT
///SPP Server - Heart beat characteristic, notify&write&read
static const uint16_t spp_heart_beat_uuid = ESP_GATT_UUID_SPP_HEARTBEAT;
static const uint8_t spp_heart_beat_val[2] = {0x00, 0x00};
static const uint8_t spp_heart_beat_ccc[2] = {0x00, 0x00};
#endif

///Full HRS Database Description - Used to add attributes into the database
static const esp_gatts_attr_db_t spp_gatt_db[SPP_IDX_NB] =
    {
        //SPP -  Service Declaration
        [SPP_IDX_SVC] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&primary_service_uuid, ESP_GATT_PERM_READ, sizeof(spp_service_uuid), sizeof(spp_service_uuid), (uint8_t *)&spp_service_uuid}},

        //SPP -  data receive characteristic Declaration
        [SPP_IDX_SPP_DATA_RECV_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write}},

        //SPP -  data receive characteristic Value
        [SPP_IDX_SPP_DATA_RECV_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_data_receive_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, SPP_DATA_MAX_LEN, sizeof(spp_data_receive_val), (uint8_t *)spp_data_receive_val}},

        //SPP -  data notify characteristic Declaration
        [SPP_IDX_SPP_DATA_NOTIFY_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_notify}},

        //SPP -  data notify characteristic Value
        [SPP_IDX_SPP_DATA_NTY_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_data_notify_uuid, ESP_GATT_PERM_READ, SPP_DATA_MAX_LEN, sizeof(spp_data_notify_val), (uint8_t *)spp_data_notify_val}},

        //SPP -  data notify characteristic - Client Characteristic Configuration Descriptor
        [SPP_IDX_SPP_DATA_NTF_CFG] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(spp_data_notify_ccc), (uint8_t *)spp_data_notify_ccc}},

        //SPP -  command characteristic Declaration
        [SPP_IDX_SPP_COMMAND_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write}},

        //SPP -  command characteristic Value
        [SPP_IDX_SPP_COMMAND_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_command_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, SPP_CMD_MAX_LEN, sizeof(spp_command_val), (uint8_t *)spp_command_val}},

        //SPP -  status characteristic Declaration
        [SPP_IDX_SPP_STATUS_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_notify}},

        //SPP -  status characteristic Value
        [SPP_IDX_SPP_STATUS_NTF_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_status_uuid, ESP_GATT_PERM_READ, SPP_STATUS_MAX_LEN, sizeof(spp_status_val), (uint8_t *)spp_status_val}},

        //SPP -  status characteristic - Client Characteristic Configuration Descriptor
        [SPP_IDX_SPP_STATUS_NTF_CFG] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(spp_status_ccc), (uint8_t *)spp_status_ccc}},

#ifdef SUPPORT_HEARTBEAT
        //SPP -  Heart beat characteristic Declaration
        [SPP_IDX_SPP_HEARTBEAT_CHAR] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_declaration_uuid, ESP_GATT_PERM_READ, CHAR_DECLARATION_SIZE, CHAR_DECLARATION_SIZE, (uint8_t *)&char_prop_read_write_notify}},

        //SPP -  Heart beat characteristic Value
        [SPP_IDX_SPP_HEARTBEAT_VAL] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&spp_heart_beat_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(spp_heart_beat_val), sizeof(spp_heart_beat_val), (uint8_t *)spp_heart_beat_val}},

        //SPP -  Heart beat characteristic - Client Characteristic Configuration Descriptor
        [SPP_IDX_SPP_HEARTBEAT_CFG] =
            {{ESP_GATT_AUTO_RSP}, {ESP_UUID_LEN_16, (uint8_t *)&character_client_config_uuid, ESP_GATT_PERM_READ | ESP_GATT_PERM_WRITE, sizeof(uint16_t), sizeof(spp_data_notify_ccc), (uint8_t *)spp_heart_beat_ccc}},
#endif
};

static bool isRun = false; 
//for  record
static audio_pipeline_handle_t pipeline_record = NULL; 
static audio_element_handle_t raw_read, rsp_filter;
static audio_element_handle_t el_record;

//static int audio_chunksize = 1*1024;
//static char *read_buf ;

static audio_event_iface_handle_t evt ;
static bool recordFinish=true;
static int audio_record_size = 0;
//static xTimerHandle blue_tx_timer;

static int pipeline_cmd = 0;

static void spp_init(void);
//static void spp_destroy(void);
static void performBtPhone();
static audio_element_err_t record_send_data(audio_element_handle_t el, char *buffer, int wanted_size);

//static int record_wr_cb(audio_element_handle_t el, char *buf, int len, TickType_t wait_time, void *ctx);
//static void blue_tx_hrate(TimerHandle_t timer);

//static void generateSinDatas(uint16_t point, uint16_t maxnum, uint16_t offset, uint16_t *pBuf);

/*function*/
static void ble_gatts_init(void);
//static void ble_gatts_deinit(void);

extern void play_mp3(void);

esp_err_t esp_bt_send_cmd(int cmd, void *data, int data_len);

void btPhoneModeStart()
{
    //generateSinDatas(512,30000,0,sin_table);
    performBtPhone();
    // play_mp3();
}

static esp_err_t _el_open(audio_element_handle_t self)
{
    ESP_LOGI(TAG, "_el_open");
    return ESP_OK;
}

static audio_element_err_t _el_read(audio_element_handle_t self, char *buffer, int len, TickType_t ticks_to_wait, void *context)
{
    ESP_LOGI(TAG, "_el_read");
    return len;
}

/*
void generateSinDatas(uint16_t point, uint16_t maxnum, uint16_t offset, uint16_t *pBuf)
{
	uint16_t i = 0;
	float x;//弧度
	float jiao; //角度 分度角
	jiao = 360.000 / point;

	for (i = 0; i < point; i++)
	{
		x = jiao * i-90;//得到角度值
		x = x * 0.01745; //角度转弧度  弧度=角度*（π/180）
		pBuf[i] = (maxnum / 2) * sin(x) + (maxnum / 2) + offset;
	}
}
*/
uint32_t micRCDNum = 0;

static int _el_process(audio_element_handle_t self, char *in_buffer, int in_len)
{
    // static int cnt=0;
    int r_size = 0;
/*    
    if(audio_record_size >= REC_DATA_SIZE){
        recordFinish = true;
		set_busy(0);
        audio_record_size = 0;
        return 0;
    }
*/
    if  (recordFinish == true) {
		set_busy(0);
        audio_record_size = 0;
        return 0;
    }
    r_size = audio_element_input(self, in_buffer, in_len);
    audio_record_size += r_size;
    int send_size = record_send_data(self,in_buffer,r_size);
    
    return send_size;
}

static audio_element_err_t _el_write(audio_element_handle_t self, char *buffer, int len, TickType_t ticks_to_wait, void *context)
{
    ESP_LOGI(TAG, "_el_write");
    return len;
}

static esp_err_t _el_close(audio_element_handle_t self)
{
    ESP_LOGI(TAG, "_el_close");
    return ESP_OK;
}

void StartRecord()
{
    char cmdBuf[8] = "Start";
    esp_bt_send_cmd('1', cmdBuf,5);
    esp_bt_send_cmd('1', cmdBuf,5);
    esp_bt_send_cmd('1', cmdBuf,5);
    
    micRCDNum = 0;
    show_trans(true);
    display_update();               			
    recordFinish=false;
    set_busy(1);
    audio_pipeline_reset_ringbuffer(pipeline_record);
    audio_pipeline_reset_elements(pipeline_record);
    audio_pipeline_resume(pipeline_record);
    ESP_LOGI(TAG, "[ pipeline_record ] audio_pipeline_resume OK");
    //xTimerStart(blue_tx_timer,1);
}

void StopRecord()
{
    char cmdBuf[8] = "Stop";
    show_trans(false);    
    display_update();               			
    recordFinish=true;
    set_busy(0);
    esp_bt_send_cmd('2',cmdBuf,4);
    esp_bt_send_cmd('2',cmdBuf,4);
    esp_bt_send_cmd('2',cmdBuf,4);
}

void performBtPhone()
{
    audio_pipeline_handle_t pipeline_player = NULL; //use for headphone
    audio_element_handle_t i2s_stream_reader, i2s_stream_writer;
    esp_err_t ret;
    int sample_rate = 44100;

    audio_element_handle_t equalizer;

    is_exit = false;
    pipeline_cmd = 0;
    
    //esp_log_level_set("*", ESP_LOG_ERROR);
    //esp_log_level_set(TAG, ESP_LOG_DEBUG);

    /*
    read_buf = (char *)malloc(audio_chunksize * sizeof(char));
    if (NULL == read_buf)
    {
        ESP_LOGE(TAG, "Memory allocation failed!");
        return;
    }
    */

    ESP_LOGI(TAG, "[ 1 ] Start codec chip");
    audio_board_handle_t board_handle = audio_board_init();
    audio_hal_ctrl_codec(board_handle->audio_hal, AUDIO_HAL_CODEC_MODE_BOTH, AUDIO_HAL_CTRL_START);	

    ESP_LOGI(TAG, "[ 2 ] Create i2s stream to read audio data from codec chip");
    i2s_stream_cfg_t i2s_cfg = I2S_STREAM_CFG_DEFAULT();
    i2s_cfg.i2s_config.sample_rate = sample_rate;
    i2s_cfg.i2s_config.channel_format = I2S_CHANNEL_FMT_ONLY_LEFT;
    i2s_cfg.type = AUDIO_STREAM_READER;
    i2s_cfg.multi_out_num = 1;
    i2s_stream_reader = i2s_stream_init(&i2s_cfg);

    ESP_LOGI(TAG, "[2.1] Create i2s stream to write data to codec chip");
    i2s_stream_cfg_t i2s_cfgw = I2S_STREAM_CFG_DEFAULT();
    i2s_cfgw.i2s_config.sample_rate = sample_rate;
    i2s_cfgw.i2s_config.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT;
    i2s_cfgw.type = AUDIO_STREAM_WRITER;
    i2s_stream_writer = i2s_stream_init(&i2s_cfgw);

    equalizer_cfg_t eq_cfg = DEFAULT_EQUALIZER_CONFIG();
    eq_cfg.samplerate= sample_rate;
    if(get_mode()== MODE_Filter_A){
        eq_cfg.set_gain =gainA; 
    }else if(get_mode()== MODE_Filter_B){
        eq_cfg.set_gain =gainB; 
    }else if(get_mode()== MODE_Filter_C){
        eq_cfg.set_gain =gainC; 
    }   
    // The size of gain array should be the multiplication of NUMBER_BAND and number channels of audio stream data. The minimum of gain is -13 dB.
    equalizer = equalizer_init(&eq_cfg);


    ESP_LOGI(TAG, "[ 3 ] Create raw to receive data");
    raw_stream_cfg_t raw_cfg = {
        .out_rb_size = 2 * 1024,
        .type = AUDIO_STREAM_READER,
    };
    raw_read = raw_stream_init(&raw_cfg);

    ESP_LOGI(TAG, "[ 2.2 ] Create filter to resample audio data");
    rsp_filter_cfg_t rsp_cfg = DEFAULT_RESAMPLE_FILTER_CONFIG();
    rsp_cfg.src_rate = sample_rate;
    rsp_cfg.src_ch = 1;
    rsp_cfg.dest_rate = 8000;
    rsp_cfg.dest_ch = 1;
    rsp_cfg.type = AUDIO_CODEC_TYPE_ENCODER;
    rsp_cfg.out_rb_size = 32*1024;
    rsp_filter = rsp_filter_init(&rsp_cfg);
    // audio_element_set_read_cb(rsp_filter,record_wr_cb,NULL);

/*
    raw_stream_cfg_t rcd_cfg = {
        .out_rb_size = 0,
        .type = AUDIO_STREAM_WRITER,
    };
    record_data = raw_stream_init(&rcd_cfg);
*/
    audio_element_cfg_t cfg = DEFAULT_AUDIO_ELEMENT_CONFIG();
    cfg.open = _el_open;
    cfg.read = _el_read;
    cfg.process = _el_process;
    cfg.write = _el_write;
    cfg.close = _el_close;
    cfg.out_rb_size = 0;
    el_record = audio_element_init(&cfg);

    ESP_LOGI(TAG, "[ 4 ] Create pipeline_record ");
    audio_pipeline_cfg_t pipeline_cfg_rcd = DEFAULT_AUDIO_PIPELINE_CONFIG();
    pipeline_record = audio_pipeline_init(&pipeline_cfg_rcd);
    mem_assert(pipeline_record);

    ESP_LOGI(TAG, "[4.1] Register all elements to pipeline_record");
    audio_pipeline_register(pipeline_record, raw_read, "raw");
    audio_pipeline_register(pipeline_record, rsp_filter, "rsp_filter");
    //audio_pipeline_register(pipeline_record, record_data, "record");
    audio_pipeline_register(pipeline_record, el_record, "el_record");

    audio_pipeline_link(pipeline_record, (const char *[]) { "raw", "rsp_filter","el_record"}, 3);

    ESP_LOGI(TAG, "Connect input ringbuffer of pipeline_record to i2s_stream_reader multi output");
    ringbuf_handle_t rb = audio_element_get_output_ringbuf(raw_read);
    if(rb == NULL){
        ESP_LOGI(TAG, "raw_read output ringbuf is null");
    }
    ret = audio_element_set_multi_output_ringbuf(i2s_stream_reader, rb, 0);

    ESP_LOGI(TAG, "Set multi output %d",ret);

    ESP_LOGI(TAG, "[4.2] Create headphone pipeline ");
    audio_pipeline_cfg_t pipeline_cfg = DEFAULT_AUDIO_PIPELINE_CONFIG();
    pipeline_player = audio_pipeline_init(&pipeline_cfg);
    mem_assert(pipeline_player);

    ESP_LOGI(TAG, "[4.3] Register all elements to headphone pipeline");
    audio_pipeline_register(pipeline_player, i2s_stream_reader, "i2s");
    audio_pipeline_register(pipeline_player, i2s_stream_writer, "i2s_stream_writer");
    ESP_LOGI(TAG, "[4.4] Link it together [codec_chip]-->i2s_stream-->filters-->i2s_stream_writer");
    
    audio_pipeline_register(pipeline_player, equalizer, "equalizer");
    audio_pipeline_link(pipeline_player, (const char *[]){"i2s", "equalizer", "i2s_stream_writer"}, 3);

    // Initialize peripherals management
    ESP_LOGI(TAG, "[ 5 ] Start peripheral");
	esp_periph_config_t periph_cfg = DEFAULT_ESP_PERIPH_SET_CONFIG();
    esp_periph_set_handle_t set = esp_periph_set_init(&periph_cfg);

    ESP_LOGI(TAG, "[5.1] Initialize Touch peripheral");
    audio_board_key_init(set);

    ESP_LOGI(TAG, "[5.2] Bluetooth init");
    spp_init();

    ESP_LOGI(TAG, "[ 6 ] Setup event listener");
    audio_event_iface_cfg_t evt_cfg = AUDIO_EVENT_IFACE_DEFAULT_CFG();
    evt_cfg.external_queue_size = 10;
    evt = audio_event_iface_init(&evt_cfg);

    ESP_LOGI(TAG, "[6.1] Listening event from all elements of pipeline");
    audio_pipeline_set_listener(pipeline_record, evt);
    audio_pipeline_set_listener(pipeline_player, evt);

    ESP_LOGI(TAG, "[6.2] Listening event from peripherals");
    audio_event_iface_set_listener(esp_periph_set_get_event_iface(set), evt);

    ESP_LOGI(TAG, "[ 7 ] Start audio_pipeline");
    audio_pipeline_run(pipeline_player);
    audio_pipeline_run(pipeline_record);
    audio_pipeline_pause(pipeline_record);
    //blue_tx_timer = xTimerCreate("blue_tx_hrate", pdMS_TO_TICKS(1000), pdTRUE, (void *)0, blue_tx_hrate);

    ESP_LOGI(TAG, "[ 8 ] Listen for all pipeline events");
    ESP_LOGI(TAG, "[ * ] Free heap size %d", esp_get_free_heap_size());
    isRun = true; 

    player_volume = 79;
    audio_hal_set_volume(board_handle->audio_hal, player_volume);
    show_sound();
	
	//audio_hal_set_mic_gain(board_handle->audio_hal,21);

	int keyOTACnt = 0;
	//bool bInOTA = false;
    while (isRun)
    {

        if ( iQuitCounter > 1)
        {
            isRun = false;
            into_sleep_mode();
            ESP_LOGD(TAG, "Goto Sleep... iQuitCounter:  %d", iQuitCounter);
            break;
        } 

        audio_event_iface_msg_t msg;
        ret = audio_event_iface_listen(evt, &msg, 20);
        if (ret == ESP_OK)
        {
            ESP_LOGI(TAG, "[ * ] Get event %d, cmd %d, data %d",msg.source_type,msg.cmd,(int)msg.data);
            /* Stop when the last pipeline element (i2s_stream_reader in this case) receives stop event */
            if (msg.source_type == AUDIO_ELEMENT_TYPE_ELEMENT && msg.source == (void *)i2s_stream_reader && msg.cmd == AEL_MSG_CMD_REPORT_STATUS && (int)msg.data == AEL_STATUS_STATE_STOPPED)
            {
                ESP_LOGW(TAG, "[ * ] Stop event received");
            }
            if (msg.source_type == AUDIO_ELEMENT_TYPE_ELEMENT && msg.source == (void *)el_record && msg.cmd == AEL_MSG_CMD_REPORT_STATUS && (int)msg.data == AEL_STATUS_STATE_FINISHED)
            {
                ESP_LOGW(TAG, "[ * ] Record finish");
                show_trans(false);
                display_update();           
                audio_pipeline_stop(pipeline_record);      			
				recordFinish=true;
				set_busy(0);        			
            }
            if (msg.source_type == PERIPH_ID_BUTTON )
            {
                iQuitCounter = 0;
                iNoKeyPressed = 0;
                // button click
                if (msg.cmd == PERIPH_BUTTON_RELEASE)
                {
                    // bluetooth/record button    
                    if ((int)msg.data == get_input_rec_id())
                    {
                        if (is_connected) {
                            if(recordFinish)
                            {
                                // start record
                                StartRecord();
                            } else {
                                // stop record, set finished flag
                                StopRecord();
                            }
                        } else {
                            recordFinish = true;
                        }
						keyOTACnt = 0;
                        ESP_LOGI(TAG, "[ * ] [Mode] rec key event");
                    }
                    // volup loop button
                    else if ((int)msg.data == get_input_volup_id())
                    {
                        ESP_LOGI(TAG, "[ * ] [Mode] vol key event");
                        if ( recordFinish )
                        {
                            player_volume += VOL_LEVEL_STEP;
                            if (player_volume > MAX_VOL_LEVEL) {
                                player_volume = 19;
                            }

    #ifdef CONFIG_ESP_ISTETHO_V1_2_BOARD                                            
                            audio_hal_set_volume(board_handle->audio_hal, player_volume);
    #else  
                            audio_hal_set_volume(board_handle->audio_hal, player_volume/100*63);
    #endif                        
                            show_sound();
                     }
						
						keyOTACnt = 0;

                        ESP_LOGI(TAG, "[ * ] Volume set to %d ", player_volume);
                    }
                    if ((int)msg.data == get_input_mode_id())
                    {
                        ESP_LOGI(TAG, "[ * ] [Mode] mode key event");
                        if(recordFinish)
                        {
#ifdef USE_EQUALIZER
                            change_mode(equalizer);
#else
                            change_mode(filter_A);
#endif    
                        }
						keyOTACnt = 0;
                    }
                    else if ((int)msg.data == get_input_play_id())
                    {
                        ESP_LOGI(TAG, "[ * ] [Mode] power key event");
						keyOTACnt++;
						if (keyOTACnt>7)
						{
							//bInOTA = true;
							keyOTACnt = 0;
						}
                    }
                }
                else if (msg.cmd == PERIPH_BUTTON_LONG_PRESSED)
                {
                    if(recordFinish)
                    {
                        if ((int)msg.data == get_input_play_id())
                        {
                            ESP_LOGI(TAG, "[ * ] [Mode] power key long press event");
                            audio_hal_ctrl_codec(board_handle->audio_hal,AUDIO_HAL_CODEC_MODE_BOTH, AUDIO_HAL_CTRL_STOP);
                            into_sleep_mode();
                            break;
                        }
                        //  wiredphone/bth_earphone 
                        /*
                        else if ((int)msg.data == get_input_rec_id())
                        {
                            ESP_LOGI(TAG, "[ * ] [Mode] rec key long press event");
                            audio_hal_set_volume(board_handle->audio_hal,0);
                            change_earphone_type();
                            audio_pipeline_stop(pipeline_player);   
                            audio_element_stop(i2s_stream_reader);
                            audio_hal_ctrl_codec(board_handle->audio_hal,AUDIO_HAL_CODEC_MODE_BOTH, AUDIO_HAL_CTRL_STOP);
                            break;
                        } */
                        //glanguage, english/chinese
                        else if ((int)msg.data == get_input_mode_id())
                        {
                            ESP_LOGI(TAG, "[ * ] [Mode] mode key long press event");
                            if(glanguage == 0){
                                glanguage = 1;
                            }else if (glanguage == 1)
                            {
                                glanguage = 0;
                            }
                            set_language(glanguage);
                            break;
                        }

                        else if (((int)msg.data == get_input_volup_id()))
                        {
                            // erase ota_data
                            /*
                            if (bInOTA)
                            {
                                erase_ota_data_partition();
                                bInOTA = false;
                                break;
                            }
                            */
                        }
                    }
                }
            }
            //audio_pipeline_resume(pipeline_player);
         }
     }

    audio_hal_ctrl_codec(board_handle->audio_hal, AUDIO_HAL_CODEC_MODE_BOTH, AUDIO_HAL_CTRL_STOP);
    ESP_LOGI(TAG, "[ * ] wired-mode over!");
    return;
    /*
    audio_hal_ctrl_codec(board_handle->audio_hal, AUDIO_HAL_CODEC_MODE_BOTH, AUDIO_HAL_CTRL_STOP);
    ESP_LOGI(TAG, "[ * ] Stop pipeline_player");
    if(pipeline_player != NULL)audio_pipeline_stop(pipeline_player);
    // ESP_LOGI(TAG, "[ 9 ] Stop audio_pipeline");
    audio_pipeline_terminate(pipeline_record);
    ESP_LOGI(TAG, "[ 9 ] Stop pipeline_player");
    audio_pipeline_terminate(pipeline_player);
   
    // vTaskDelay(pdMS_TO_TICKS(100));
    // ESP_LOGI(TAG, "[ 9 ] Stop raw_read");
    audio_pipeline_unregister(pipeline_record, raw_read);
    // ESP_LOGI(TAG, "[ 9 ] Stop rsp_filter");
    audio_pipeline_unregister(pipeline_record, rsp_filter);
    ESP_LOGI(TAG, "[ 9 ] audio_pipeline_unregister i2s_stream_reader");
    audio_pipeline_unregister(pipeline_player, i2s_stream_reader);

    ESP_LOGI(TAG, "[ 9 ] audio_pipeline_unregister filter_A");
#ifdef USE_EQUALIZER
    audio_pipeline_unregister(pipeline_player, equalizer);
#else
    audio_pipeline_unregister(pipeline_player, filter_A);
#endif

    ESP_LOGI(TAG, "[ 9 ] audio_pipeline_unregister i2s_stream_writer");
    audio_pipeline_unregister(pipeline_player, i2s_stream_writer);

    ESP_LOGI(TAG, "[ * ] audio_pipeline_remove_listener");
    audio_pipeline_remove_listener(pipeline_record);
    audio_pipeline_remove_listener(pipeline_player);

    ESP_LOGI(TAG, "[ * ] esp_periph_stop_all");
    esp_periph_set_stop_all(set);
    ESP_LOGI(TAG, "[ * ] audio_event_iface_remove_listener");
    audio_event_iface_remove_listener(esp_periph_set_get_event_iface(set), evt);

    audio_event_iface_destroy(evt);

    audio_pipeline_deinit(pipeline_record);
    audio_pipeline_deinit(pipeline_player);
    audio_element_deinit(i2s_stream_writer);
    audio_element_deinit(raw_read);
    audio_element_deinit(i2s_stream_reader);
    audio_element_deinit(rsp_filter);
#ifdef USE_EQUALIZER    
    audio_element_deinit(equalizer);
#else    
    audio_element_deinit(filter_A);
#endif    
    audio_board_deinit(board_handle);
    esp_periph_set_destroy(set);
    spp_destroy();
    //xTimerDelete(blue_tx_timer,1);
    free(read_buf);
    */
}

uint8_t  ntf_value_p[264];
audio_element_err_t record_send_data(audio_element_handle_t el, char *buffer, int wanted_size){
    uint8_t total_num = 0;
    uint8_t current_num = 0;

    uint8_t *temp = (uint8_t *)buffer;
    int readSize = wanted_size;

    if ( ( (micRCDNum++) % 16) == 0) {
        ESP_LOGE("Mic", "sendsize=%d.", micRCDNum);
    } 


    while((readSize > 0)&&(is_connected))
    {
        #ifdef SUPPORT_HEARTBEAT
        if(!enable_heart_ntf){
            ESP_LOGE(GATTS_TABLE_TAG, "%s do not enable heartbeat Notify\n", __func__);
            break;
        }
        #endif
        if(!enable_data_ntf){
            unsigned portBASE_TYPE uxHighWaterMark=uxTaskGetStackHighWaterMark( NULL );
            ESP_LOGW(TAG,"Main task left heap size %d.\r\n",uxHighWaterMark);
            //ESP_LOGI(TAG, "[ * ] Free heap size %d",esp_get_free_heap_size());
            ESP_LOGE(GATTS_TABLE_TAG, "%s do not enable data Notify\n", __func__);
            break;
        }
        
        if(readSize <= (spp_mtu_size - 3)){
            esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_DATA_NTY_VAL],readSize, temp, false);
        }else if(readSize > (spp_mtu_size - 3)){

            if((readSize%(spp_mtu_size - 7)) == 0){
                total_num = readSize/(spp_mtu_size - 7);
            }else{
                total_num = readSize/(spp_mtu_size - 7) + 1;
            }

            current_num = 1;
            /*
            ntf_value_p = (uint8_t *)malloc((spp_mtu_size-3)*sizeof(uint8_t));
            if(ntf_value_p == NULL){
                ESP_LOGE(GATTS_TABLE_TAG, "%s malloc.2 failed, size %d\n", __func__,spp_mtu_size-3);
                //  if failure, set finished flag  
                show_trans(false);    
                display_update();               			
                recordFinish=true;
                set_busy(0);                
                break;
            }
            */
            while(current_num <= total_num){
                if(current_num < total_num){
                    ntf_value_p[0] = '#';
                    ntf_value_p[1] = '#';
                    //ntf_value_p[2] = micRCDNum / 256;
                    ntf_value_p[2] = micRCDNum % 256;
                    //ntf_value_p[2] = total_num;
                    ntf_value_p[3] = current_num;
                    memcpy(ntf_value_p + 4,temp + (current_num - 1)*(spp_mtu_size-7),(spp_mtu_size-7));
                    esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_DATA_NTY_VAL],(spp_mtu_size-3), ntf_value_p, false);
                }else if(current_num == total_num){
                    ntf_value_p[0] = '#';
                    ntf_value_p[1] = '#';
                    //ntf_value_p[2] = micRCDNum / 256;
                    ntf_value_p[2] = micRCDNum % 256;
                    //ntf_value_p[2] = total_num;
                    ntf_value_p[3] = current_num;
                    memcpy(ntf_value_p + 4,temp + (current_num - 1)*(spp_mtu_size-7),(readSize - (current_num - 1)*(spp_mtu_size - 7)));
                    esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_DATA_NTY_VAL],(readSize - (current_num - 1)*(spp_mtu_size - 7) + 4), ntf_value_p, false);
                }
                vTaskDelay(10 / portTICK_PERIOD_MS);
                current_num++;
            }
            //free(ntf_value_p);
        }
        break;
    }
    return readSize;
}

/*
int record_wr_cb(audio_element_handle_t el, char *buf, int len, TickType_t wait_time, void *ctx)
{
    static int cnt = 0;
    cnt++;
    if(cnt > 100){
        cnt = 0;
        ESP_LOGE(TAG, "write call back, size %d ", len);
    }
    return len;
}

static void blue_tx_hrate(TimerHandle_t timer){
    static bool state = true;
    state = (!state);
    show_trans(state);
    display_update();
}
*/

esp_err_t esp_bt_send_cmd(int cmd, void *data, int data_len)
{

	if (recordFinish == false)
	{
		return ESP_GATT_OK;
	}

    if (!enable_data_ntf)
    {
        //ESP_LOGI(TAG, "enable_data_ntf not set! ");
		return ESP_GATT_OK;
    }

    uint8_t cmdBuf[20];
    int sendLen = (data_len > 19) ? 19 : data_len;
    cmdBuf[0] = cmd;
    memcpy(cmdBuf + 1, data, sendLen);
    esp_err_t rtn = esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_STATUS_NTF_VAL], sendLen + 1, cmdBuf, false);
    if (ESP_OK != rtn) {
        ESP_LOGE(TAG, "esp_bt_send_cmd  failure..., restart iStethoscope" );
        
		// iQuitCounter = 101;
        is_connected = false;
        enable_data_ntf = false;
        enable_cmd_ntf = false;
       
        #ifdef SUPPORT_HEARTBEAT
        enable_heart_ntf = false;
        heartbeat_count_num = 0;
        #endif
        if (!is_exit)
        {
            esp_ble_gap_start_advertising(&spp_adv_params);
        }
        show_bluetooth_connect(false);
        display_update();
        //esp_bt_send_cmd(PERIPH_BLUETOOTH_DISCONNECTED,0,0);

    }
    return rtn;
}

static uint8_t find_char_and_desr_index(uint16_t handle)
{
    uint8_t error = 0xff;

    for (int i = 0; i < SPP_IDX_NB; i++)
    {
        if (handle == spp_handle_table[i])
        {
            return i;
        }
    }

    return error;
}

static bool store_wr_buffer(esp_ble_gatts_cb_param_t *p_data)
{
    temp_spp_recv_data_node_p1 = (spp_receive_data_node_t *)malloc(sizeof(spp_receive_data_node_t));

    if (temp_spp_recv_data_node_p1 == NULL)
    {
        ESP_LOGI(GATTS_TABLE_TAG, "malloc error %s %d\n", __func__, __LINE__);
        return false;
    }
    if (temp_spp_recv_data_node_p2 != NULL)
    {
        temp_spp_recv_data_node_p2->next_node = temp_spp_recv_data_node_p1;
    }
    temp_spp_recv_data_node_p1->len = p_data->write.len;
    SppRecvDataBuff.buff_size += p_data->write.len;
    temp_spp_recv_data_node_p1->next_node = NULL;
    temp_spp_recv_data_node_p1->node_buff = (uint8_t *)malloc(p_data->write.len);
    temp_spp_recv_data_node_p2 = temp_spp_recv_data_node_p1;
    memcpy(temp_spp_recv_data_node_p1->node_buff, p_data->write.value, p_data->write.len);
    if (SppRecvDataBuff.node_num == 0)
    {
        SppRecvDataBuff.first_node = temp_spp_recv_data_node_p1;
        SppRecvDataBuff.node_num++;
    }
    else
    {
        SppRecvDataBuff.node_num++;
    }

    return true;
}

static void free_write_buffer(void)
{
    temp_spp_recv_data_node_p1 = SppRecvDataBuff.first_node;

    while (temp_spp_recv_data_node_p1 != NULL)
    {
        temp_spp_recv_data_node_p2 = temp_spp_recv_data_node_p1->next_node;
        free(temp_spp_recv_data_node_p1->node_buff);
        free(temp_spp_recv_data_node_p1);
        temp_spp_recv_data_node_p1 = temp_spp_recv_data_node_p2;
    }

    SppRecvDataBuff.node_num = 0;
    SppRecvDataBuff.buff_size = 0;
    SppRecvDataBuff.first_node = NULL;
}

static void print_write_buffer(void)
{
    temp_spp_recv_data_node_p1 = SppRecvDataBuff.first_node;

    while (temp_spp_recv_data_node_p1 != NULL)
    {
        esp_log_buffer_char(TAG, (char *)(temp_spp_recv_data_node_p1->node_buff), temp_spp_recv_data_node_p1->len);
        //uart_write_bytes(UART_NUM_0, (char *)(temp_spp_recv_data_node_p1->node_buff), temp_spp_recv_data_node_p1->len);
        temp_spp_recv_data_node_p1 = temp_spp_recv_data_node_p1->next_node;
    }
}

#ifdef SUPPORT_HEARTBEAT
void spp_heartbeat_task(void *arg)
{
    uint16_t cmd_id;

    for (;;)
    {
        vTaskDelay(50 / portTICK_PERIOD_MS);
        if (xQueueReceive(cmd_heartbeat_queue, &cmd_id, portMAX_DELAY))
        {
            while (1)
            {
                heartbeat_count_num++;
                vTaskDelay(5000 / portTICK_PERIOD_MS);
                if ((heartbeat_count_num > 3) && (is_connected))
                {
                    esp_ble_gap_disconnect(spp_remote_bda);
                }
                if (is_connected && enable_heart_ntf)
                {
                    esp_ble_gatts_send_indicate(spp_gatts_if, spp_conn_id, spp_handle_table[SPP_IDX_SPP_HEARTBEAT_VAL], sizeof(heartbeat_s), heartbeat_s, false);
                }
                else if (!is_connected)
                {
                    break;
                }
            }
        }
    }
    vTaskDelete(NULL);
}
#endif

/*
char ota_SSID[18] = {'0'};
char ota_Pwd[18] = {'0'};
char ota_Url[64] = {'0'};
char strOtaSite[10] = "joekung";
char strOtaFile[20] = "iStetho8388.bin";
*/

void spp_cmd_task(void *arg)
{
    uint8_t *cmd_id;
    for (;;)
    {
        vTaskDelay(50 / portTICK_PERIOD_MS);
        if (xQueueReceive(bt_cmd_queue, &cmd_id, portMAX_DELAY))
        {
            ESP_LOGI(GATTS_TABLE_TAG, "received cmd:");
            esp_log_buffer_char(GATTS_TABLE_TAG, (char *)(cmd_id), strlen((char *)cmd_id));
            if (cmd_id[0] == '1')
            {
                if (recordFinish == true) {
                    micRCDNum = 0;
                    pipeline_cmd = 1;
                    // start record
                    StartRecord();
                }
            }
            else if (cmd_id[0] == '2')
            {
                if (recordFinish == false) {

                    pipeline_cmd = 2;
                    // stop record, set finished flag
                    StopRecord();
                 }
           }
            else if (cmd_id[0] == '3')
            {
                pipeline_cmd = 3;
/*                
                ESP_LOGI(TAG, "Wifi: Pwd");
				char* strPwd = (char*)cmd_id + 1;
                strcpy(ota_Pwd, strPwd);
				ESP_LOGI(GATTS_TABLE_TAG, "Config --> PWD: %s.\n", ota_Pwd);
                set_wifiConfig("", ota_Pwd, "");
*/                
            }
            else if (cmd_id[0] == '4')
            {
                pipeline_cmd = 4;
/*                
                strcpy(strOtaSite, (char*)cmd_id + 1);
                sprintf(ota_Url, "http://%s.imwork.net/iStetho/%s",strOtaSite, strOtaFile);
                ESP_LOGI(GATTS_TABLE_TAG, "Config --> OtaUrl: %s.\n", ota_Url);
                set_wifiConfig("", "", ota_Url);    
*/                
            }
            else if (cmd_id[0] == '5')
            {
                pipeline_cmd = 5;
/*                
                strcpy(strOtaFile, (char*)cmd_id + 1);
                sprintf(ota_Url, "http://%s.imwork.net/iStetho/%s",strOtaSite, strOtaFile);
                ESP_LOGI(GATTS_TABLE_TAG, "Config --> OtaUrl: %s.\n", ota_Url);
                set_wifiConfig("", "", ota_Url);    
*/                
            }
            else if (strcmp((char *)cmd_id, "exit") == 0)
            {
                //is_exit = true;
                ESP_LOGI(TAG, "[ * ] spp_cmd_task exit");
            }
            free(cmd_id);
            if (is_exit)
            {
                break;
            }
        }
    }
    vTaskDelete(NULL);
}

static void spp_task_init(void)
{

    #ifdef SUPPORT_HEARTBEAT
    cmd_heartbeat_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(spp_heartbeat_task, "spp_heartbeat_task", 2048, NULL, 10, NULL);
    #endif

    bt_cmd_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(spp_cmd_task, "spp_cmd_task", 2048, NULL, 10, NULL);
}

static void gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param)
{
    esp_err_t err;
    ESP_LOGD(GATTS_TABLE_TAG, "GAP_EVT, event %d", event);

    switch (event)
    {
    case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT:
        esp_ble_gap_start_advertising(&spp_adv_params);
        break;
    case ESP_GAP_BLE_ADV_START_COMPLETE_EVT:
        //advertising start complete event to indicate advertising start successfully or failed
        if ((err = param->adv_start_cmpl.status) != ESP_BT_STATUS_SUCCESS)
        {
            ESP_LOGE(GATTS_TABLE_TAG, "Advertising start failed: %s\n", esp_err_to_name(err));
        }
        break;
    default:
        break;
    }
}

static void gatts_profile_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{
    esp_ble_gatts_cb_param_t *p_data = (esp_ble_gatts_cb_param_t *)param;
    uint8_t res = 0xff;

    //ESP_LOGI(GATTS_TABLE_TAG, "gatts_profile_event = %d",event);
    switch (event)
    {
    case ESP_GATTS_REG_EVT:
        ESP_LOGI(GATTS_TABLE_TAG, "%s %d\n", __func__, __LINE__);
        esp_ble_gap_set_device_name(SAMPLE_DEVICE_NAME);

        ESP_LOGI(GATTS_TABLE_TAG, "%s %d\n", __func__, __LINE__);
        esp_ble_gap_config_adv_data_raw((uint8_t *)spp_adv_data, sizeof(spp_adv_data));

        ESP_LOGI(GATTS_TABLE_TAG, "%s %d\n", __func__, __LINE__);
        esp_ble_gatts_create_attr_tab(spp_gatt_db, gatts_if, SPP_IDX_NB, SPP_SVC_INST_ID);
        break;
    case ESP_GATTS_READ_EVT:
        res = find_char_and_desr_index(p_data->read.handle);
        if (res == SPP_IDX_SPP_STATUS_NTF_VAL)
        {
            //TODO:client read the status characteristic
        }
        break;
    case ESP_GATTS_WRITE_EVT:
    {
        res = find_char_and_desr_index(p_data->write.handle);
        if (p_data->write.is_prep == false)
        {
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_WRITE_EVT : handle = %d\n", res);

            //  AFB1 APP write VAL
            if (res == SPP_IDX_SPP_DATA_RECV_VAL)       
            {
                #ifdef SPP_DEBUG_MODE
                esp_log_buffer_char(GATTS_TABLE_TAG, (char *)(p_data->write.value), p_data->write.len);
                #else
                //uart_write_bytes(UART_NUM_0, (char *)(p_data->write.value), p_data->write.len);
                esp_log_buffer_char(TAG, (char *)(p_data->write.value), p_data->write.len);
                #endif
            }
            // ABF2 APP enanle Data NTF (AUTO-READ)
            else if (res == SPP_IDX_SPP_DATA_NTF_CFG)
            {
                if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x01) && (p_data->write.value[1] == 0x00))
                {
                    enable_data_ntf = true;
                }
                else if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x00) && (p_data->write.value[1] == 0x00))
                {
                    enable_data_ntf = false;
                }
                is_connected = true;
                ESP_LOGI(GATTS_TABLE_TAG, "enable_data_ntf = %d\n", enable_data_ntf);
            }
            //  ABF3 APP write cmd
            else if (res == SPP_IDX_SPP_COMMAND_VAL)
            {
                uint8_t *spp_cmd_buff = NULL;
                spp_cmd_buff = (uint8_t *)malloc((spp_mtu_size - 3) * sizeof(uint8_t));
                if (spp_cmd_buff == NULL)
                {
                    ESP_LOGE(GATTS_TABLE_TAG, "%s malloc failed\n", __func__);
                    break;
                }
                memset(spp_cmd_buff, 0x0, (spp_mtu_size - 3));
                memcpy(spp_cmd_buff, p_data->write.value, p_data->write.len);
                xQueueSend(bt_cmd_queue, &spp_cmd_buff, 10 / portTICK_PERIOD_MS);
            }
            // ABF4 APP enanle CMD NTF (AUTO-READ)
            else if (res == SPP_IDX_SPP_STATUS_NTF_CFG)
            {
                if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x01) && (p_data->write.value[1] == 0x00))
                {
                    enable_cmd_ntf = true;
                }
                else if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x00) && (p_data->write.value[1] == 0x00))
                {
                    enable_cmd_ntf = false;
                }
                is_connected = true;
                ESP_LOGI(GATTS_TABLE_TAG, "enable_cmd_ntf = %d\n", enable_cmd_ntf);
            }
            #ifdef SUPPORT_HEARTBEAT
            //  ABF5 APP write CCC
            else if (res == SPP_IDX_SPP_HEARTBEAT_CFG)
            {
                if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x01) && (p_data->write.value[1] == 0x00))
                {
                    enable_heart_ntf = true;
                }
                else if ((p_data->write.len == 2) && (p_data->write.value[0] == 0x00) && (p_data->write.value[1] == 0x00))
                {
                    enable_heart_ntf = false;
                }
            }
            //  ABF5 APP write VAL
            else if (res == SPP_IDX_SPP_HEARTBEAT_VAL)
            {
                if ((p_data->write.len == sizeof(heartbeat_s)) && (memcmp(heartbeat_s, p_data->write.value, sizeof(heartbeat_s)) == 0))
                {
                    heartbeat_count_num = 0;
                }
            }
            #endif
            else
            {
                //TODO:
            }
        }
        else if ((p_data->write.is_prep == true) && (res == SPP_IDX_SPP_DATA_RECV_VAL))
        {
            ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_PREP_WRITE_EVT : handle = %d\n", res);
            store_wr_buffer(p_data);
        }
        break;
    }
    case ESP_GATTS_EXEC_WRITE_EVT:
    {
        ESP_LOGI(GATTS_TABLE_TAG, "ESP_GATTS_EXEC_WRITE_EVT\n");
        if (p_data->exec_write.exec_write_flag)
        {
            print_write_buffer();
            free_write_buffer();
        }
        break;
    }
    case ESP_GATTS_MTU_EVT:
        spp_mtu_size = p_data->mtu.mtu;
        break;
    case ESP_GATTS_CONF_EVT:
        break;
    case ESP_GATTS_UNREG_EVT:
        break;
    case ESP_GATTS_DELETE_EVT:
        break;
    case ESP_GATTS_START_EVT:
        break;
    case ESP_GATTS_STOP_EVT:
        break;
    case ESP_GATTS_CONNECT_EVT:
        spp_conn_id = p_data->connect.conn_id;
        spp_gatts_if = gatts_if;
        is_connected = true;
        memcpy(&spp_remote_bda, &p_data->connect.remote_bda, sizeof(esp_bd_addr_t));
        #ifdef SUPPORT_HEARTBEAT
        uint16_t cmd = 0;
        xQueueSend(cmd_heartbeat_queue, &cmd, 10 / portTICK_PERIOD_MS);
        #endif
        show_bluetooth_connect(true);
        display_update();
        esp_bt_send_cmd(PERIPH_BLUETOOTH_CONNECTED,0,0);
        break;
    case ESP_GATTS_DISCONNECT_EVT:
        ESP_LOGW(GATTS_TABLE_TAG, "gatts_profile_event = ESP_GATTS_DISCONNECT_EVT");
        is_connected = false;
        enable_data_ntf = false;
        enable_cmd_ntf = false;
       
        #ifdef SUPPORT_HEARTBEAT
        enable_heart_ntf = false;
        heartbeat_count_num = 0;
        #endif
        if (!is_exit)
        {
            esp_ble_gap_start_advertising(&spp_adv_params);
        }
        show_bluetooth_connect(false);
        display_update();
        esp_bt_send_cmd(PERIPH_BLUETOOTH_DISCONNECTED,0,0);
        break;
    case ESP_GATTS_OPEN_EVT:
        break;
    case ESP_GATTS_CANCEL_OPEN_EVT:
        break;
    case ESP_GATTS_CLOSE_EVT:
        break;
    case ESP_GATTS_LISTEN_EVT:
        break;
    case ESP_GATTS_CONGEST_EVT:
        break;
    case ESP_GATTS_CREAT_ATTR_TAB_EVT:
    {
        ESP_LOGI(GATTS_TABLE_TAG, "The number handle =%x\n", param->add_attr_tab.num_handle);
        if (param->add_attr_tab.status != ESP_GATT_OK)
        {
            ESP_LOGE(GATTS_TABLE_TAG, "Create attribute table failed, error code=0x%x", param->add_attr_tab.status);
        }
        else if (param->add_attr_tab.num_handle != SPP_IDX_NB)
        {
            ESP_LOGE(GATTS_TABLE_TAG, "Create attribute table abnormally, num_handle (%d) doesn't equal to HRS_IDX_NB(%d)", param->add_attr_tab.num_handle, SPP_IDX_NB);
        }
        else
        {
            memcpy(spp_handle_table, param->add_attr_tab.handles, sizeof(spp_handle_table));
            esp_ble_gatts_start_service(spp_handle_table[SPP_IDX_SVC]);
        }
        break;
    }
    default:
        break;
    }
}

static void gatts_event_handler(esp_gatts_cb_event_t event, esp_gatt_if_t gatts_if, esp_ble_gatts_cb_param_t *param)
{
     //ESP_LOGI(GATTS_TABLE_TAG, "EVT %d, gatts if %d", event, gatts_if);

    /* If event is register event, store the gatts_if for each profile */
    if (event == ESP_GATTS_REG_EVT)
    {
        if (param->reg.status == ESP_GATT_OK)
        {
            spp_profile_tab[SPP_PROFILE_APP_IDX].gatts_if = gatts_if;
        }
        else
        {
            ESP_LOGI(GATTS_TABLE_TAG, "Reg app failed, app_id %04x, status %d\n", param->reg.app_id, param->reg.status);
            return;
        }
    }

    do
    {
        int idx;
        for (idx = 0; idx < SPP_PROFILE_NUM; idx++)
        {
            if (gatts_if == ESP_GATT_IF_NONE || /* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */
                gatts_if == spp_profile_tab[idx].gatts_if)
            {
                if (spp_profile_tab[idx].gatts_cb)
                {
                    spp_profile_tab[idx].gatts_cb(event, gatts_if, param);
                }
            }
        }
    } while (0);
}

static void ble_gatts_init(void)
{
    esp_err_t ret;
	esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
	bt_cfg.controller_task_stack_size = 16*1024;
    bt_cfg.mode =ESP_BT_MODE_BLE;
    ret = esp_bt_controller_init(&bt_cfg);
    if (ret) {
        ESP_LOGE(GATTS_TABLE_TAG, "%s init controller failed: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    ret = esp_bt_controller_enable(bt_cfg.mode);
    if (ret) {
        ESP_LOGE(GATTS_TABLE_TAG, "%s enable controller failed: %s\n", __func__, esp_err_to_name(ret));
        return;
    }

    ESP_LOGI(GATTS_TABLE_TAG, "%s init bluetooth\n", __func__);
    ret = esp_bluedroid_init();
    if (ret) {
        ESP_LOGE(GATTS_TABLE_TAG, "%s init bluetooth failed: %s\n", __func__, esp_err_to_name(ret));
        return;
    }
    ret = esp_bluedroid_enable();
    if (ret) {
        ESP_LOGE(GATTS_TABLE_TAG, "%s enable bluetooth failed: %s\n", __func__, esp_err_to_name(ret));
        return;
    }
	
	ret = esp_ble_gatts_register_callback(gatts_event_handler);
    if (ret)
    {
        ESP_LOGE(GATTS_TABLE_TAG, "gatts register error, error code = 0x%x", ret);
        return;
    }
    ret = esp_ble_gap_register_callback(gap_event_handler);
    if (ret)
    {
        ESP_LOGE(GATTS_TABLE_TAG, "gap register error, error code = 0x%x", ret);
        return;
    }
    ret = esp_ble_gatts_app_register(ESP_SPP_APP_ID);
    if (ret)
    {
        ESP_LOGE(GATTS_TABLE_TAG, "gatts app register error, error code = 0x%x", ret);
        return;
    }
}

void spp_init()
{
    ble_gatts_init();
    spp_task_init();
}

/*
void spp_destroy()
{

    char *spp_cmd_buff = NULL;
    spp_cmd_buff = (char *)malloc(5 * sizeof(char));
    if (spp_cmd_buff != NULL)
    {
        char *cmd = "exit";
        memset(spp_cmd_buff, 0x0, 5);
        memcpy(spp_cmd_buff, cmd, 4);
        xQueueSend(bt_cmd_queue, &spp_cmd_buff, 10 / portTICK_PERIOD_MS);
    }
    if (is_connected)
    {
        int cnt = 0;
        esp_ble_gatts_close(spp_gatts_if, spp_conn_id);
        while (is_connected)
        {
            portTickType xDelay = pdMS_TO_TICKS(100);
            vTaskDelay(xDelay);
            cnt++;
            if (cnt > 20)
            {
                ESP_LOGE(TAG, "[ * ] Wait disconnect timeout");
                break;
            }
        }
        esp_ble_gap_disconnect(spp_remote_bda);
    }

    if (spp_handle_table[SPP_IDX_SVC] != 0)
    {
        esp_ble_gatts_stop_service(spp_handle_table[SPP_IDX_SVC]);
        esp_ble_gatts_delete_service(spp_handle_table[SPP_IDX_SVC]);
    }
    esp_ble_gatts_app_unregister(ESP_SPP_APP_ID);

    esp_err_t error = ESP_OK;
    ESP_LOGI(TAG, "[ * ] esp_bluedroid_disable");
    error = esp_bluedroid_disable();
    ESP_LOGI(TAG, "[ * ] rst %d ", error);
    ESP_LOGI(TAG, "[ * ] esp_bluedroid_deinit");
    error = esp_bluedroid_deinit();
    ESP_LOGI(TAG, "[ * ] rst %d ", error);
    ESP_LOGI(TAG, "[ * ] esp_bt_controller_disable");
    error = esp_bt_controller_disable();
    ESP_LOGI(TAG, "[ * ] rst %d ", error);
    ESP_LOGI(TAG, "[ * ] esp_bt_controller_deinit");
    error = esp_bt_controller_deinit();
    ESP_LOGI(TAG, "[ * ] rst %d ", error);
    ESP_LOGI(TAG, "[ * ] esp_bt_controller_mem_release");
    //error = esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
    ESP_LOGI(TAG, "[ * ] rst %d ,esp_bt_controller_get_status %d", error, esp_bt_controller_get_status());
}
*/